#ifndef KD_DEBUG_TRACER_H
#define KD_DEBUG_TRACER_H

#include "../RayTracePro/RayTracePro.h"

namespace RayTracePro
{
#ifndef DBG_KD_TRAVERSE_STATS
#define DBG_KD_TRAVERSE_STATS 1
#endif
	template<bool pred>
	class MeshTracer
	{
	public:
		MeshGeometry * mesh;
		inline bool Trace(DifferentialGeometry & inter, const MeshFace & face, const Ray & ray, const RayDifferential & diffIn, float & t) const
		{
			const int mod3[] = {0,1,2,0,1};
			int u = mod3[face.ProjectionAxis + 1];
			int v = mod3[face.ProjectionAxis + 2];
			float invNdotD = 1.0f / (face.PlaneU * ray.Direction[u] + face.PlaneV * ray.Direction[v] + ray.Direction[face.ProjectionAxis]);
			float tplane = -(face.PlaneU * ray.Origin[u] + face.PlaneV * ray.Origin[v] + ray.Origin[face.ProjectionAxis] + face.PlaneD) * invNdotD;
			if (tplane < ray.tMin-Epsilon || tplane > ray.tMax+Epsilon)
				return false;
			float hitU = ray.Origin[u] + ray.Direction[u] * tplane;
			float hitV = ray.Origin[v] + ray.Direction[v] * tplane;
			
			float beta = face.K_beta_u * hitU + face.K_beta_v * hitV + face.K_beta_d;
			if (beta < -Epsilon)
				return false;
			float gamma = face.K_gamma_u * hitU + face.K_gamma_v * hitV + face.K_gamma_d;
			if (gamma < -Epsilon)
				return false;
			if (beta + gamma > 1.0f + Epsilon)
				return false;
			if (pred)
				return true;
			inter.Init();
			inter.Position[u] = hitU;
			inter.Position[v] = hitV;
			inter.Position[face.ProjectionAxis] = ray.Origin[face.ProjectionAxis] + ray.Direction[face.ProjectionAxis] * tplane;
			float alpha = 1.0f - beta - gamma;
			Vec3 tmp;
			Vec3::Scale(inter.Normal, mesh->Normals[face.Normal1], alpha);
			Vec3::Scale(tmp, mesh->Normals[face.Normal2], beta);
			inter.Normal += tmp;
			Vec3::Scale(tmp, mesh->Normals[face.Normal3], gamma);
			inter.Normal += tmp;

			Vec2 tmpVec2;
			if (face.TexCoord1 != -1)
			{
				inter.UV.x = mesh->TexCoords[face.TexCoord1].x * alpha;
				inter.UV.y = mesh->TexCoords[face.TexCoord1].y * alpha;
				inter.UV.x += mesh->TexCoords[face.TexCoord2].x * beta;
				inter.UV.y += mesh->TexCoords[face.TexCoord2].y * beta;
				inter.UV.x += mesh->TexCoords[face.TexCoord3].x * gamma;
				inter.UV.y += mesh->TexCoords[face.TexCoord3].y * gamma;
			}
			inter.dNdxContinuation = (void*)&face;
			t = tplane;
			return true;
		}
	};

	template<typename T, typename Tracer, bool pred, bool useMailbox>
	bool TraverseKdTreeDbg(RayTraceSystem * system, 
		const Tracer & tracer, 
		DifferentialGeometry & rs, 
		KdTree<T> & tree, 
		const Ray & ray, 
		const RayDifferential & rayDiff,
		float & t,
		Array<int, true> & traversedNodes)
	{
#ifdef DBG_KD_TRAVERSE_STATS
		int nodeTraversed = 0;
		int leafTraversed = 0;
		int emptyLeafTraversed = 0;
		int triangleTraversed = 0;
#endif
		static volatile long rayIdAllocator = 1;
		float tmin, tmax;
		if (!RayBBoxIntersection(tree.Bounds, ray, tmin, tmax))
			return false;
		tmax += Epsilon;
		tmin -= Epsilon;
		bool retVal = false;
		KdTraverseItem<T> stack[TraverseStackLength];
		KdNode<T> * node = tree.Nodes.Buffer();
		Ray traceRay = ray;
		int stackPtr = 0;
		traceRay.tMax = tmax;
		traceRay.tMin = tmin;
		rs.Normal = traceRay.Direction;
		rs.Position.SetZero();
		rs.Geometry = 0;
		unsigned int rayId = 1;
		if (useMailbox)
			rayId = _InterlockedExchangeAdd(&rayIdAllocator, 1);	
		while (node)
		{
			int nodeId = node - tree.Nodes.Buffer();
			traversedNodes.Add(nodeId);
			if (nodeId == 154689)
				printf("this is it!");
			if (FloatSmaller(traceRay.tMax, tmin))
				break;
			if (FloatLarger(tmin, tmax))
				break;
#ifdef DBG_KD_TRAVERSE_STATS
			nodeTraversed ++;
#endif
			if (node->GetIsLeaf())
			{
				DifferentialGeometry inter;
				inter.Init();
				int count = node->GetElementCount();
#ifdef DBG_KD_TRAVERSE_STATS
				leafTraversed ++;
				triangleTraversed += count;
				if (count == 0)
					emptyLeafTraversed ++;
#endif
				MailboxedElement<T>** startElem = node->Elements;
				if (count == 1) startElem = &node->OneElement;
				for (MailboxedElement<T>** elem = startElem; elem < startElem + count; elem++)
				{
					MailboxedElement<T>* curElem = *elem;
					// check mailbox
					bool mailboxHit = false;
					if (useMailbox)
					{
						curElem->Lock.Lock();
						mailboxHit = (curElem->RayId == rayId);
						if (mailboxHit)
						{
							if (curElem->t >= traceRay.tMin - Epsilon && curElem->t <= traceRay.tMax + Epsilon)
							{
								if (pred)
								{
									curElem->Lock.Unlock();
									return true;
								}
								else
									retVal = true;
								rs = inter;
								t = curElem->t;
								traceRay.tMax = curElem->t;
							}
						}
						curElem->Lock.Unlock();
					}
					if (!mailboxHit)
					{
						float it = -1.0f;
						bool hit = false;
						if (hit = tracer.Trace(inter, curElem->Element, traceRay, rayDiff, it))
						{
							if (pred)
								return true;
							else
								retVal = true;
							if (FloatLarger(ray.tMax, it) || Vec3::Dot(inter.Normal, traceRay.Direction) < Vec3::Dot(rs.Normal, traceRay.Direction))
							{
								rs = inter;
								traceRay.tMax = it;
								t = it;
							}
							if (useMailbox)
							{
								curElem->Lock.Lock();
								curElem->RayId = rayId;
								curElem->t = it;
								curElem->Intersection = inter;
								curElem->Lock.Unlock();
							}
						}
					}
				}
				if (stackPtr)
				{
					KdTraverseItem<T> & item = stack[--stackPtr];
					node = item.node;
					tmax = item.tMax;
					tmin = item.tMin;
				}
				else
					break;
			}
			else // Interior node
			{
				int axis = (int)(node->GetSplitDimension());
				bool coplanar = abs(ray.Direction[axis]) < Epsilon;

				float tplane;
				if (!coplanar)
					tplane = (node->SplitPlane - ray.Origin[axis]) * ray.ReciprocalDirection[axis];
				KdNode<T>* node1, *node2;
				float rayOrigin = ray.Origin[axis];
				if (rayOrigin <= node->SplitPlane)
				{
					node1 = node + 1;
					node2 = (KdNode<T>*)((char*)tree.Nodes.Buffer() + node->GetOffset());
				}
				else
				{
					node1 = (KdNode<T>*)((char*)tree.Nodes.Buffer() + node->GetOffset());
					node2 = node + 1;
				}

				if (coplanar||(tplane > tmax+Epsilon || tplane < 0))
				{
					node = node1;
				}
				else if (tplane < tmin - Epsilon)
				{
					node = node2;
				}
				else
				{
					KdTraverseItem<T> toTraverse;
					toTraverse.node = node2;
					toTraverse.tMin = tplane;
					toTraverse.tMax = tmax;
					stack[stackPtr++] = toTraverse;
					node = node1;
					tmax = tplane;
				}
			}
		}
#ifdef DBG_KD_TRAVERSE_STATS
		if (system->DebugPrint)
		{
			printf("Nodes traversed: %d\n", nodeTraversed);
			printf("leaves traversed: %d\n", leafTraversed);
			printf("empty leaf traversed: %d\n", emptyLeafTraversed);
			printf("triangles traversed: %d\n", triangleTraversed);
		}
#endif
		return retVal;
	}
}

#endif